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EXECUTIVE SUMMARY 


Task Overview 

Grant NAG 9-326 supported 24 months of research comprising two related tasks 
in grasping using dexterous robot hands. This section of the report summarizes the 
objectives and status of each task. 

Introduction 

There are four steps that must be taken to ensure the grasp of an arbitrary 
object. The first step is the pre-grasp phase where approach to the object is planned, 
but no contact is made. Next the object’s position and orientation, or pose , within 
the grasping region of the hand must be known to position and close the hand in 
an appropriate manner. An added benefit from knowing the object pose has to do 
with manipulation of the object. Using the sensor data can aid in grasping the object 
in such a way which will either not require further manipulation or will make the 
manipulation of the object easier. Third, the hand must close in a way to create an 
envelope in which the object cannot escape. The forth step is to use the information 
from tactile, force, and vision sensors to determine whether a stable grasp has been 
achieved. 

It is clearly understood that to achieve the grasp of an object requires low- 
level control to move, monitor, and compensate the finger motions. To this end, 
a method to calculate the joint coordinates for prescribed finger positions and a 
method of determining the pose of the object within the grasping region of the hand 
are necessary. 

The Journal of Robotics and Automation has been used as a model for style and 
format. 
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Generalized Inverse Kinematics 

Controlling the motion of each finger of a dexterous hand will require the 
inverse kinematic solution for the hand; i.e., to find the joint coordinates of each 
finger given a fingertip pose. The desired fingertip positions are pre-selected contact 
points on the target object. These contact points can be determined based on task 
constraints. For example, the vision sensors locate and track the target object. Based 
on the object’s geometry, contact points are selected which are inputs to the inverse 
kinematic algorithm. 

Proposed solutions to the inverse kinematic problem have used analytical, 
iterative, and knowledge-based systems. Numerical iterative techniques are useful 
because of their generality in the sense that the same basic algorithm can be applied 
to many different manipulators. This generality allows the user to quickly modify 
the algorithm for the desired dexterous hand. The solution method consists of 
writing the finger equations using homogeneous transformations, specifying desired 
finger contact points, and determination of the joint coordinates by minimizing the 
difference between the desired fingertip pose and the actual fingertip pose. 

The finger equations can be written by treating each finger as an independent 
manipulator. A finger may be modeled as a series of mechanical linkages connected by 
prismatic or revolute joints. Each link is then defined in terms of special parameters 
known as the Denavit-Hartenberg (link) parameters. 

An equation may be obtained relating the joint variables in the link matrices to 
the task space coordinates. Then, through a minimization process, the joint variables 
are found for the required position and orientation. This minimization is accomplished 
using a nonlinear least-squares technique. 

The algorithm presented has been tested on a three-fingered dexterous hand. 
Given a target object and selected contact points, feasible solutions were obtained. In 
general, the limitations in the algorithm are that the fingers must have either revolute 
or prismatic joints and are open loop kinematic chains. This algorithm can aid in 
calculating the inverse kinematic solution for the various dexterous hands available on 
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site, or the ones currently being developed, and will provide an alternative to solving 
for the solution to these hands analytically. 

Pose Estimation of a Pre-Grasped Object 

Knowledge about the object, such as the position and orientation, must be 
obtained that will allow a grasp strategy to be formulated. The problem of grasping 
objects by dexterous hands has been widely considered. In most studies, the object 
is well defined by vision sensors and/or by tactile sensors, and the grasp strategy 
uses the data from these sensors. The use of vision sensors may be limited because 
the object may be hidden from view by the robot’s arms or other objects. In the 
space environment, tactile sensors cannot be used for the pre-grasp phase because 
the object cannot be touched prior to grasping or the object will float away. The 
pose estimation problem is to determine the pre-grasp position and orientation of an 
object using local, noncontact sensors. 

The pose estimation problem is divided into two subtasks: the forward model 
and the inverse model. The forward model of the pose estimation problem assumes 
the object pose and finger joint angles are known and solves for the sensor parameters. 
The inverse model assumes sensory data is available, the finger joint angles are known, 
and uses this information to find the object pose. Results from the forward model 
will enable verification of the inverse model. 

To develop the equations for pose estimation, a mathematical model of the sen- 
sor characteristics is necessary for software implementation. The type of noncontact 
sensors that are planned for use on the EVA Retriever are optical reflectance proxim- 
ity sensors. Due to the complexity of modelling this type of nonlinear sensor, it was 
decided to implement an ideal sensor. 

The pose estimation equations were written using the concept of triangulation. 
An example of a two-fingered hand surrounding a sphere is presented. By choosing 
the object to be a sphere, only the position of the object frame was important. 
Results show that a minimum of three sensors are needed to find the position of the 
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sphere. More complex shapes can also be implemented, but this will require more 
computation to model them. 

The algorithms for inverse kinematics and pose estimation were developed 
assuming several simplifications. In retrospect, the investigation has provided a 
software shell which can be easily upgraded to take into account more complex shapes 
and sensor models. 

Work In Progress 

A strategy termed the Grasp Algorithm is proposed which utilizes the inverse 
kinematics and pose estimation software to provide low-level control of a dexterous 
hand when grasping an object. This algorithm is currently being implemented on site 
using computer animation software and should be available for viewing by December, 
1990. 

Bibliography Generated from the Grant 

To date, one paper titled “Pose Estimation of a Pre-Grasped Object Using 
Local Sensors on a Dexterous Robotic Hand” was generated from grant support. It 
will be presented at the Fifth International Conference on CAD/CAM Robotics and 
Factories of the Future that will take place in December, 1990. 



5 


INTRODUCTION 

Current work at NASA’s Johnson Space Center includes developing an au- 
tonomous robot to perform tasks such as object rescue and retrieval, space shuttle 
experimentation, satellite repair, and space station construction. Dexterous end effec- 
tors with sensors can help the robot perform this large variety of tasks. Of the tasks 
previously mentioned, object rescue and retrieval is considered the most challenging 
and one of the primary reasons for developing an autonomous robot [1]. Object res- 
cue and retrieval requires locating and tracking the object, movement towards the 
object, and grasping and maintaining a stable grasp of the object [1]. Stable grasp 
must be ensured to avoid having the object escape and drift into space. The EVA 
Retriever, shown in Figure 1, supplied with dexterous end effectors is currently under 
development to perform this task. 

There are four steps that must be taken to ensure the grasp of an arbitrary 
object. The first step is the pre-grasp phase where approach to the object is planned, 
but no contact is made. Next the object’s position and orientation, or pose, within 
the grasping region of the hand must be known to position and close the hand in an 
appropriate manner. An added benefit from knowing the object pose has to do with 
manipulation of the object. Using the sensor data, the object can be grasped in such a 
way which will either not require further manipulation or will make the manipulation 
of the object easier. Third, the hand must close in a way to create an envelope in 
which the object cannot escape (this may only be possible for objects smaller than 
the palm width of the hand). Finally, using multiple sensor information, from tactile, 
force, and vision sensors, determine whether a stable grasp has been achieved. 

To achieve the grasp of an object requires low-level control to move, monitor, 
and compensate the finger motions. To this end, a method to calculate the joint 
coordinates for prescribed finger positions and a method of determining the pose of 
the object within the grasping region of the hand are necessary. 
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Figure 1. The EVA Retriever [1], 
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Kinematic Analysis of Manipulators 

A manipulator may be modeled as a series of mechanical linkages connected 
by, but not limited to, prismatic or revolute joints. Each link is defined in terms 
of special parameters known as the Denavit-Hartenberg (link) parameters. The 
spatial position of a link frame relative to another frame is expressed as a 4 x 4 
homogeneous transformation matrix [2]. Using this information permits defining a 
generic manipulator whose configuration is based on user-supplied link parameters. 

To control the motion of each finger of a dexterous hand will require the inverse 
kinematic solution for the hand; i.e., to find the joint coordinates of each finger of a 
dexterous hand for a given fingertip pose. Proposed solutions to the inverse kinematic 
problem have used analytical, iterative, and knowledge-based systems. An analytical 
closed-form solution [2, 3] is advantageous in that it allows the joint variables of a 
particular manipulator to be easily found; on the other hand since only certain classes 
of manipulators allow a closed-form solution, it is very difficult to solve explicitly for 
the joint variables without having some geometric intuition about the manipulator, 
and the solution only applies to the particular manipulator. 

Numerical iterative techniques are useful because of their generality in the 
sense that the same basic algorithm can be applied to many different manipulators, 
including kinematically redundant manipulators and manipulators with less than six 
degrees-of-freedom. Some techniques are more useful than others for certain classes of 
manipulators. For example, Poon and Lawrence (1988) present an algorithm that is 
useful for functionally partitionable manipulators, which are manipulators where each 
link has its own task and the joints are controlled independently. These manipulators, 
such as the Unimation PUMA and Unimate, Cincinnati Milacron, and the Stanford 
manipulators, can be partitioned into major and minor linkage sets. The major 
linkage controls the end effector position and the minor linkage changes the end 
effector orientation. Furthermore, most iterative techniques utilize the manipulator 
Jacobian, as in Goldenberg, Benhabib, and Fenton (1985). A necessary condition 
for this method to function is that the Jacobian must be nonsingular and can 
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therefore be inverted. A problem arises at certain points in the joint space of the 
manipulator, termed the joint-space singularities , where the Jacobian matrix loses 
rank and becomes ill-conditioned [6]. Goldenberg, Benhabib, and Fenton overcome 
the problem of singularities by utilizing the generalized inverse, or pseudoinverse . 

A knowledge-based solution using neural networks is proposed by Guez and 
Ziauddin (1988). The drawback to this method is that it is computationally expensive 
in that the network must be trained to conform to the manipulator used, versus simply 
changing certain parameters as in most iterative techniques. 

Pose Estimation of a Pre-Grasped Object 

Knowledge about the object, such as the position and orientation, must be 
obtained that will allow a grasp strategy to be formulated. The problem of grasping 
objects by dexterous hands has been widely considered [8, 9, 10, 11]. In most studies, 
the object is well defined by vision sensors and/or by tactile sensors, and the grasp 
strategy uses the data from these sensors. The use of vision sensors may be limited by 
the object being hidden from view by the robot’s arms or other objects. In the space 
environment, tactile sensors cannot be used because the object cannot be touched 
prior to grasping or the object will float away. The pose estimation problem is to 
determine the pre-grasp position and orientation of an object using local, noncontact 
sensors. Noncontact sensors , such as proximity sensors, do not require touch to 
operate. 

The sensors that will be modeled are optical reflectance proximity sensors. This 
type of sensor will be used on the EVA Retriever. The advantages of this type of 
sensor are: good size/range ratio, low cost, good reliability, simple to use, and low 
sensitivity to disturbances by using synchronous modulation or pulsed emmission 
[12]. Balaure describes and gives the mathematical model of an optical proximity 
sensor (1986). This model will be useful when implementing a real-world sensor in 
software. Limited research has been performed on using proximity sensors mounted 
on a dexterous end effector to determine the position and orientation of an object [14]. 
Furhman and Kanade apply the concept of triangulation to determine the position and 
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orientation of an object’s surface using a multilight source proximity sensor (1984). 
Romiti and Raparelli (1987) present a method of finding the position and orientation 
of a dexterous hand based on proximity sensor data. 
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KINEMATIC ANALYSIS OF MANIPULATORS 

Robot manipulators can be considered to consist of a series of rigid links 
connected together by a series of joints. The relationship between adjacent links 
is described by 4 x 4 homogeneous transformations whose elements are dependent 
on the link parameters known as the Denavit-Hartenberg parameters. The product 
of these 4x4 matrices gives the pose of a selected frame relative to a reference 
frame. This chapter describes the kinematics of a manipulator using the concept of 
homogeneous transformations. 

Homogeneous Transformations 

Paul (1981) describes the homogeneous transformations that define the trans- 
lation and orientation of a coordinate frame. 

The transformation corresponding to a translation in the direction of vector 
v — ai + bj + ck relative to a reference frame is given as a 4 x 4 homogeneous matrix 

‘1 0 0 a' 

Trans (a,b,c) = q 0 1 c ' (*) 

° ° ° 1 

Orientation is frequently specified as the product of rotations about the x, y, 
and 2 axes, as shown in Figure 2. The Euler transformation describes the orientation 
of a coordinate frame in terms of a rotation <p about the 2 axis, then a rotation # 

t # // 

about the new y axis, y , and finally a rotation about the new 2 axis, 2 , of ip. Thus, 
the Euler transformation is given by the product of the three rotation matrices 

Euler(V>, #, VO = Rot( 2 ,<^)Rot(y, 0 )Rot( 2 ,VO 

cos <p cos # cos ip — sin <p sin % 
sin <j) cos # cos ip + cos (p sin % 

— sin 9 cos ip 
0 

— cos <p cos 6 sin ip — sin V>cos ip cos <£ sin# 0 

— sin <p cos 9 sin ip + sin <p cos 4 1 sin <p sin 0 0 

sin# sin ip cos# 0 

0 0 1 


Euler(</!>, #, VO = 
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Figure 2. Euler angles [2]. 


If a coordinate frame is translated in the direction given by v and then rotated 
by angles <f>, 6, and 'ijj as described above, the composite homogeneous transformation 
matrix T which represents the position and orientation of the resulting coordinate 
frame is 

T = Trans(a,6, c)Euler(<^,$, (3) 

Denavit-Hartenberg Parameters 

A serial link manipulator can be considered to consist of a sequence of links 
connected together by actuated joints, and for an n degree-of- freedom manipulator, 
there are n + 1 links and n joints. 

Assigning coordinate frames to each link of the manipulator permits finding the 
link parameters. Coordinate frames can be assigned to the links according to the 
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scheme presented in Paul (1981), pp. 50-52. In general, each link of the manipulator 
will have assigned to it a series of link parameters, known as the Denavit-Hartenberg 
parameters, which characterize each link and give the relationship between connected 
links. The parameters are the distance d and angle 0 between adjacent links, and the 
length a and twist angle a of a single link (Figures 3, 4, and 5). 

Joint n Joint n + 1 



Figure 3. The length a and the twist a of a link [2]. 


Depending on what type of joint a link has will determine the joint variable. 
For a revolute joint, the joint variable is 6. For a prismatic joint, the distance d is 
the joint variable. Figure 4 shows the link parameters for revolute joints and Figure 
5 shows the link parameters for a prismatic joint. 

Having defined the link coordinate frames, the relation between the successive 
frames n — 1 and n is the matrix A n defined as 

A n = Rot(~,# n )Trans(0,0,dJTrans(a n ,0,0)Rot(x,a„) (4) 
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Joint n 


11 i A 


Link n 


e. 


arameters of a prismatic joint [2]. 
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Relation Between Joint and Task Space Coordinates 

An equation may be obtained relating the joint variables 6 and d in the A 
matrices to the task space coordinates, which are the required position and orientation 
of the selected frame given by the T matrix. For example, if matrix Ai describes the 
position and orientation of the first link relative to the base frame and matrix A 2 
describes the position and orientation of the second link relative to the first, then the 
position and orientation of the second link relative to the base frame is the matrix 
T 2 . This is represented as 

T2 = A1A2. 

More links may be added, and their position and orientation relative to the base frame 
can be found. Thus, the transformation representing the position and orientation of 
the end effector with respect to the base frame for an n degree-of-freedom manipulator 
is 

T n(q) = II Ai(q) = f 0 001) ^ 

where n, o, a are orientation vectors, p is the position vector, and q is the vector 
consisting of the joint variables [5]. Equation 6 in matrix form is 

Ox O'x Px 
°y a y Py 

o z a z p z 

0 0 1 _ 

Inverse Kinematic Problem 

Generally the target position and orientation of the end effector frame for an 
n degree-of-freedom manipulator is given as the matrix T^, where the superscript 
t designates target pose. The problem of finding the joint variables to achieve this 
pose is known as the inverse kinematic problem (IKP). In other words, the IKP is 
the determination of the vector q composed of the joint variables that will yield the 
end effector target transformation T^. 

The inverse kinematics of a manipulator can be solved by finding the closed- 
form solution. For example, for a six degree-of-freedom manipulator such as the 


Tn(q) - 


Tlx 

n y 

n z 

0 



16 


Stanford manipulator, the transformation from the base frame to the end effector 
frame yields a T n matrix of the form 

Tg = A1A2A3A4A5A6. ( 7 ) 

Equation 7 is premultiplied six times by the A matrix inverses where, after each 
multiplication, an equation is obtained for a joint variable in terms of the other 
variables [ 2 ]. 

Conversely, the IKP may be solved iteratively until the actual end effector 
transformation is coincident with the target transformation matrix T^. The next 
chapter describes a numerical algorithm based on this idea for solving the IKP. 



GENERALIZED INVERSE KINEMATICS 


1 


Two of the most widely used methods of solving the inverse kinematic problem 
(IKP) are either analytical or numerical. One of the major drawbacks to solving 
the IKP analytically is that the solution pertains to only a particular manipulator, 
in contrast to the numerical solution which can be easily modified to find the joint 
variables of different manipulators. This chapter presents a numerical scheme used 
to solve the IKP. 

Numerical Solution 

As previously explained, the IKP is the determination of a vector q, consisting 
of the n joint variables, that will yield the end effector target transformation T^. The 
objective is to minimize the difference between the actual end effector transformation 
T“ and the target transformation through minimization of the residual functions 
of position and orientation. Given the target transformation matrix as 

o* a* p‘\ 

0 0 1 / 

o a a a p a \ 

0 0 1 ) 

then the functions of residual position are defined as 

t a 
r x = Px - P X 

r y =pl - Py ( 8 ) 

t a 
r z — Pz ~ Pz 

and the residual orientation functions are 

- 4> a 

r e = e 1 - e a 


and as 


rjyt 


n 

0 


rpa 

n 


n 


r i< =4* - i' a 


( 9 ) 
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where, from Paul (1981), <f>, 9 , and ip are defined as 

{ 0, if a x and a y = 0; 

atan2(|a y |, |fljj ) 4" , if flj and d y 0, 

atan2(a J/ , a x ), otherwise, 

0 = atan2(a r cos <p + a y sin <f>, a z ), 

ip = atan2( —n x sin <p -f n y cos <p , ~°x sin <p + o y cos <p) . 


( 10 ) 


Goldenberg, Benhabib, and Fenton (1985) define the residual function vector 


r = r(q) as 


r = (r x r y r z r^r & r^) 


where ( r x r y r z ) and (r^r^r^) stand for the residual position and the residual orien- 
tation, respectively. The r vector represents the six independent constraints on the 
n unknown components of the vector q. The target vector q is obtained when the 
residual functions are a minimum and T“ is equal to T^; i.e., 


r (q) = 0. 


( 11 ) 


Properties of Solutions 

Existence of Solutions 

There are several conditions necessary for which solutions to the IKP exist. If 
the desired end effector position is outside the work envelope of the manipulator, then 
a solution clearly does not exist. The work envelope of a manipulator is defined as the 
locus of points in R 3 that can be reached by the manipulator as the joint variables 
are swept through their respective limits [6]. 

In addition, even if the desired position is within the work envelope, the required 
end effector orientation may be such that it causes one or more of the joint variable 
limits to be exceeded. Goldenberg, Benhabib, and Fenton (1985) suggest a method to 
prevent from converging to a nonfeasible solution: 1) impose upper and lower limits 
on the joint variable displacements such that 

q' < q < q“ 


(12) 
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where and q u designate the lower and upper limits, respectively, checking the 
solution at each iteration against the limits and correcting those variable values which 
are out-of-range to their nearest limit; or 2) reduce the iteration step size with a 
criteria for reduction which is dependent on the numerical method chosen for finding 
the joint variables. 

In general, for an arbitrary end effector position and orientation to be possible, 
the number of unknowns in q must be at least equal to the number of independent 
constraints, or 

for a general end effector pose n > 6. (13) 

Equation 13 is a necessary but not sufficient condition for the existence of a solution 
to the IKP. Also, the end effector position must be within the work envelope of the 
manipulator, and the desired orientation must be such that none of the joint variable 
limits are exceeded. 

Uniqueness of Solutions [6] 

When solutions are obtained, they are usually not unique. For example, a 
kinematically redundant manipulator, where n > 6, can typically have infinitely 
many solutions because it has more degrees of freedom than necessary to establish 
an arbitrary end effector pose. Even when the manipulator is not kinematically 
redundant, there may be times when the solution to the IKP is not unique. Several 
distinct solutions can arise when the size of the joint-space work envelope is sufficiently 
large. Figure 6 shows two solutions for a nonredundant robot placing a tool at point 
p . The two solutions are referred to as the elbow-up and elbow-down solution. As 
shown, both solutions give the same pose for the end effector, but their joint space 
coordinates are clearly distinct. 

Algorithm 

The residual functions are minimized using a nonlinear least-squares technique 
based on the Levenberg-Marquardt method. The least-squares algorithm is part 
of the IMSL Library of Mathematical Routines and is labeled DUNLSF. Figure 7 
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elbow up 



Figure 6. Example showing the multiple solutions for a nonredundant robot [6]. 


shows a flowchart of the algorithm to solve a general inverse kinematic problem. The 
first step (box 1) is to determine the link parameters for the manipulator. Next, 
input the target position and orientation of the manipulator end effector frame as 
data arrays POS(3) and EULER(3), as shown in box 2. In box 3, input an initial 
guess of the joint coordinates in the array labeled XGUESS. In box 4, initialize 
the parameters required by the IMSL software (LDFJAC,M,N,IPARAM). Next, a 
subroutine labeled FORWARD, which is the driver for DUNLSF, is called by the main 
program to find the joint coordinates (box 5). FORWARD contains the nonlinear r 
functions to be minimized, and DUNLSF performs the minimization process. Figure 
8 shows a flowchart for subroutine FORWARD. When a solution is obtained, the 
program verifies whether the joint limits have been exceeded using a user-defined 
subroutine called CONSTRAINT (box 6). If one or more joint coordinates exceeds 
their respective limits, this subroutine replaces it with its nearest limit. Then the 
iteration process begins again. Once all of the joint coordinates have been found, the 
results are printed (box 7). 
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The next two sections present examples of using the algorithm. 

Stanford Manipulator 

The analytical solution is presented and its results will be compared to the 
results obtained numerically. 

Analytical Solution 

A sketch of the Stanford Manipulator is shown in Figure 9 with coordinate 
frames assigned to the links. As shown, the manipulator consists of five revolute 
joints with joint variables 8 \, 62 , # 4 , # 5 , and and a prismatic joint with variable 
^ 3 . The link parameters are shown in Table 1 . 


Table 1 . Link parameters for the Stanford Manipulator [ 2 ]. 


Link 

Variable 

a 


1 

.*1 

-90° 

0 

0 

2 

02 

90° 

0 

d 2 

3 

dz 

0° 

0 

d 3 

4 

04 

-90° 

0 

0 

5 

05 

O 

O 

0 

0 

6 

8 $ 

0° 

0 

0 


The following abbreviations will be used for the sine and cosine of the angle 6 

sin 8 X - S i 
cos 1 = C i 
sin (Oi + 8 j) — S ij 
cos(i 9{ + 8j) - C ij. 



determine link 
parameters 



Flowchart for the inverse kinematics algorith 





















25 


Again, the target pose of the end effector frame relative to the base frame is 
given by equation 7, and is rewritten below 


Tg = A1A2A3A4A5A6. 


(14) 


The procedure Paul uses to solve for the joint variables is to obtain six matrix 
equations by successively multiplying equation 14 by the A matrix inverses. After 
each multiplication, an equation relating a joint variable to the remaining joint 
variables is obtained. Paul’s solution will be summarized below. 

Given the target pose 


T 


t 

6 


n x 

o x 

&x 

Pi 

n y 

°y 

(ly 

Py 

n z 

o z 

a z 

Pz 

0 

0 

0 

1 


the joint coordinate 0\ is 


9\ — tan 1 



tan 


di 

d= \/r 2 — d 2 2 


where + is for a right-hand coordinate system and - is for a left-hand coordinate 
system and 

p x = r cos <f> 


p y — r sin <j> 
r = +y/p x 2 + p y 2 

4> — tan -1 ^ . 

These trigonometric substitutions for p x and p y are necessary because the equation 
used to obtain 9\ is of the form 


“SlPz + Cl Py — ^2. 

The remaining joint coordinates are 

0 2 = tan -1 ° 1P * + SlPy 
Pz 

dz = S 2 (C lPx 4- Sipj,) + C 2 P z 
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a . —l Sia x + C\a y 

u 4 = tan — 

G 2 ( G 1 CL x ~f" S l U ^ ) S 2 Cl z 


if 6 $ > 0 


or 


#4 = #4 + 7T if #5 < 0. 


For $5 = 0, the manipulator becomes degenerate with both the axes of joint 4 and 
joint 6 aligned. Then, 6 4 can be assigned any value and is usually assigned a value 
which will result in an overall manipulator configuration which is close to the previous 
configuration in terms of total distance traveled by the manipulator to achieve the 
new pose. The equation for #5 is 


0 5 


, C4[C2(Cia x + Si ay) - S2a^] + S4[-Sia x + C\a y ] 
1 an — — 

S 2 (Cia x + Sia^) + C<ia z 


and finally, Q§ is 


where 


0 0 — t an 


Se 

C 6 


S6 = ”C5{C4[C2(CiO x + Si Oy) — S 2 O z \ + S4[-SiO x + C 1 O y ] } 


+ S5{S2(CiO a: + SlOy) + C2 O z } 


C'6 — -“S4[C2(CiO x + Si O y ) — S20 z ] + C4 [ — SiO x + ClOy]. 

Numerical Solution 

The left-hand-side of equation 14 is determined using the known target end 
effector pose with equation 3. Because this manipulator has six degrees-of-freedom, 
both the position and orientation required for the end effector can be specified. In 
equation 14, the product of the A matrices on the right is the actual transformation 
matrix T£. These matrices are unknown because the joint variables are unknown. 
Example 

Given an arbitrary Euler angle set of 

<p = 45° 

6 = 17.6° 
iP - 27.2° 
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a position vector with values 

p x — 25.1 inches 
p y = 32.7 inches 
p z = 22.6 inches, 

and c/2 = 8 inches, then the target transformation matrix is 



0.27626 

-0.93700 

0.21381 

25.1 

0.92269 

0.32083 

0.21381 

32.7 

-0.26893 

0.13821 

0.95319 

22.6 

0 

0 

0 

1 


Imposing the joint constraints listed below 


— 7 r 

< 

& 1 

< 

7T 

— 7T / 2 

< 

00 

< 

7r/2 

dz 

> 

0 



— 7 r 

< 

0 4 

< 

7T 

— 7t/2 

< 

h 

< 

7r/2 

— 7T 

< 

$6 

< 

0 


yields the results shown in Table 2, which compares the resulting joint variables for 
the given T l 6 matrix using both the analytical and numerical methods. As Table 2 
shows, the results for all joint variables using both methods are equal. 


Table 2. Analytical versus numerical results for the Stanford Manipulator. 


Link 

Variable 

Analytical 

Result 

Numerical 

Result 

1 

0i 

0.7208 rad 

0.7208 rad 

2 


1.0612 rad 

1.0612 rad 

3 

^3 

46.326 in 

46.326 in 

4 

e 4 

3.113 rad 

3.113 rad 

5 

0s 

0.7548 rad 

0.7548 rad 

6 

0 6 

-2.585 rad 

-2.585 rad 
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In summary, the analytical and numerical solutions for a six degree-of-freedom 
manipulator have been shown. The numerical solution is a more simple approach and 
equations are easily obtained using the concept of homogeneous transformations. By 
selecting appropriate joint variable limits, the solution will converge to the analytical 
solution. 


Dexterous Hand 


To solve the inverse kinematics of a dexterous hand, each finger is treated as an 
independent manipulator. A coordinate frame for the palm, or the palm frame , can 
be selected to serve as the reference frame from which all measurements are made. 
As a case study, the University of Minnesota hand, shown in Figure 10, is used. 
Analytical Solution 

The closed-form solution is presented in Koehler and Donath (1988). They 
assume that the desired location of the fingertips, with respect to the reference frame, 
are contact points on the object that have been determined based on task constraints; 
e.g., to provide a stable grasp [3]. Since the fingers have only three degrees-of- freedom, 
only the required fingertip position (or orientation, but not both) can be specified. 
Given these three locations, the objective is to find the finger joint angles. 

Numerical Solution 

An equation is necessary which relates the desired fingertip positions to the 
palm frame. As in the analytical solution, the desired fingertip positions are selected 
contact points on the targeted object. 

The transformation from the palm frame to the zth contact point, T^, is 
illustrated in Figure 11 and is given by 


T p/ = T p/ T/fci (15) 

where T^' is the transformation from the palm frame to the base frame of finger i, 
and is the transformation from the base frame of finger i to the contact point 
corresponding to finger i. The transformation T c f \ is the product 

n, = ft A > 

i= i 


( 16 ) 



where = 45°. The first subscript 

designates finger number, the second 
designates link number. 


Figure 10. Coordinate frames for the University of Minnesota Hand [3]. 
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Figure 11. Transformation from the palm frame to the contact point. 


where n designates the degrees-of- freedom of finger i. 

Another equation for is 

rr\Object 

pf pf J_ object 

and is also illustrated in Figure 11. The transformations given in equation 17 are 
known assuming the pose of the object frame relative to the palm frame is known 
and contact points on the object, one for each finger, have been specified. By equating 
(15) to (17), the equation for finding the joint variables of each finger is 

rpfb t rpC t rpObjed rpCj 

Pf X A ~ p/ object' 

The objective is then to minimize the difference between both sides of the equation 
above. 
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Example 


If the target object is a rectangular bar with the base frame aligned to the palm 
frame as shown in Figure 12, the matrix which describes the transformation from 
palm frame to the bar frame T^ T is 


T b p a f = Trans(2, 0, 2) 


10 0 2 
0 10 0 
0 0 12 
0 0 0 1 



Figure 12. Bar frame with respect to the palm frame. 


Selecting contact points on the bar as shown in Figure 13, the transformations 
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from the bar frame to the each contact point, T^ r , are 

'10 0 

Tfe r — Trans(0, —1,-5) = <j J ° 

0 0 0 

"10 0 

= Trans(0, 1,.5) = Q J J 
0 0 0 
"10 0 

Tfcar = Trans(0,0, —.5) = g J J 

0 0 0 




Figure 13. Finger contact points. 


Figure 14 gives a graphical representation of the link parameters and Table 3 
lists the D-H parameters for the fingers. Note that all of the fingers have been chosen 
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Table 3. Link parameters for the Minnesota hand. 


Link 

Variable 

a 

a 

a 

1 


90° 

i” 

0 

2 

e 2 

0° 

i” 

0 

3 

$3 

0° 

i” 

0 



Figure 14. Link parameters for a finger on the Minnesota hand. 


to have the same D-H parameters. 

Using these parameters, the A matrix for each link of a finger can be determined 
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Table 4. Assumed values for the dexterous hand. 


Variable 

Value 

Xf 

i” 

VS 

i" 

Z J 

4" 

Xg 

1” 

~g 

1” 

0 

45° 


using equation 4. For example, the A matrix for link 1 is 

Ai = Rot(zo, ^i)Trans(0, 0, 0)Trans(ai, 0, 0)Rot(xo? 90°) 

cos 6\ 0 sin $2 0 

_ sin 6\ 0 — cos 8\ aisin#i 

”010 0 

0 0 0 1 

Next the transformation from the palm frame to the base frame of each finger 
T pf 1 ' 2 ' 3 is determined using Figure 10. For fingers 1 and 2, this transformation is 
the result of a translation from the palm frame to the finger base frame (0 link 
frame) a distance Xf along the x~axis, ±yf along the y-axis, and zj along the z-axis. 
Mathematically, this is expressed as 

T p/ 12 = Tr ans (x f ,±y f ,z f ) 

1 0 0 Xf 

0 1 0 ±y f 

“001 z f 
0 0 0 1 

where —yf corresponds to finger 1 , and +yf corresponds to finger 2. For finger 3, 
the transformation is the result of a translation x g along the x-axis, a translation z g 


along the z-axis, and a rotation about the y-axis. This is expressed as 

T p/ 3 = Trans(z g ,0,z s )Rot(y,d>) 

cos <f> 0 sin 4> x g 

0 10 0 

— sin 4> 0 cos <j> z g 

0 0 0 1 

The values for x g , yj, zj, and <f> are listed in Table 4. Finally, impose the 
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joint limits listed below: for fingers 1 and 2 

— tt/6 < 6 \ < 7r/6 

— 7t/3 < 0 2 < 0 

— 7t/2 < 0 3 < 0 

and for finger 3 

— 7T / 6 < #i < 7 t/6 

0 < $2 < ?r/3 
0 < 0 3 < 7 t/2 

The resulting hand configuration is shown in Figure 15. 

Discussion 

A numerical scheme was presented to solve the inverse kinematic problem for 
a general manipulator and adopted to a dexterous hand. The scheme was based 
on minimization of the residual functions of position and orientation. As examples, 
the inverse kinematics for the Stanford Manipulator and the University of Minnesota 
Hand were determined. 

A benefit of using a numerical scheme versus an analytical method is that 
it allows quick modification to conform to the desired hand. The scheme may be 
modified to correspond to most recently developed dexterous hands; i.e., that have 
revolute or prismatic joints, and fingers that are open kinematic chains. The routine 
can be modified by changing the number of fingers, the number of links per finger (not 
counting link 0), the D-H parameters, and the joint variable constraints. Contact 
points on the object corresponding to each finger must also be defined. Depending on 
the number of degrees-of-freedom for the fingers will determine the number of residual 
functions that are minimized. 
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POSE ESTIMATION OF A P RE-GRASPED OBJECT 

This chapter presents a method of calculating the pre-grasp pose of an object 
using data from local, noncontact sensors. Local refers to sensors that are mounted 
on the end effector and for the case of dexterous hands, the sensors are mounted on 
the fingers. Noncontact sensors are sensors that do not require physical contact to 
operate, such as proximity sensors. 

The pose estimation problem is divided into two subtasks: the forward model 
and the inverse model. The forward model is analogous to the forward kinematic 
solution of a manipulator where the joint coordinates are known and the question 
is to find the end effector pose. The forward model of the pose estimation problem 
assumes the object pose and finger joint angles are known and solves for the sensor 
parameters. The inverse model assumes sensory data is available, the finger joint 
angles are known, and uses this information to find the object pose. Results from the 
forward model will enable verification of the inverse model. 

A discussion on a real-world sensor follows which exhibits the mathematical 
equations for a fiber optic reflectance sensor. Due to the complexity of the real-world 
model, an ideal sensor is defined and used in the pose estimation algorithm. 

Pose Estimation Analysis 

Forward Model 

The forward model determines the sensor parameters based on a known object 
pose. The sensor parameters to determine are: r, the radial (sensor-to-trigger point) 
distance, and the angles the trigger point may be offset from the sensor axes. 
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sensor model is used (this model will be discussed in the next section) the offsets 7 
and (3 are unknowns in the Tensor matrices. 

r 

T sensor is the transformation from the sensor frame to the palm frame given by 

rpp/ trpfbrplinkrpSenSOT] — 1 

^sensor “ l X p/ X /6 X link 1 • ) 

is the transformation from the palm frame to the finger base frame, T is 
the transformation from the finger base frame to the link frame, and T tf n n r i» the 
transformation from the link frame to the sensor frame. These transformations are 
known because the finger joint angles and the sensor locations are known. 

The transformation from the palm frame to the object frame, T° b j/ ect , is 

T° p b / ect = Trans (X 0 ,Y 0 ,Z 0 ) Euler(<M,^). 

This matrix is known because the position and orientation of the object are known. 
Inverse Model 

The inverse model determines the object pose given the sensor data. Using 
a scheme based on triangulation, which is a method used to optically locate points 
in space [15], the necessary equations are developed. From Figure 17 the following 
transformation equations are written 


T 


object 

Pf 


= T 


sensor 

Pf 



1 


... = T 


sensor tr pobject 
pf x sensor , 


for i sensors. It then follows that 


rpsensorirpobject rpsensorinpobject 

X pf x sensor i X p/ X sensor 2 


rpSensorirpobject rp^en3ur3rj\0 

pf ^sensori "p/ x sensor^ 


'\3en30r3rjyobject 


( 21 ) 


rr\sensor 

L pf 


irpobject 
1 sensor 1 


iect 


rriSenSOT^rryobje 

~~ ^ pf x sensor ,* 
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Figure 17. Triangulation scheme. 


The known and unknowns in the matrices above are: the T s *p soT matrices are known 
because the inverse kinematics are known, r (sensor-to-trigger point distance) for each 
sensor is known because the sensors have been triggered. However, the angular offsets 
are not known. Again, if the sensors used are ideal sensors, then for each equation 
in ( 21 ) there are four unknowns (7 and (3 are unknown on both sides). Therefore 
at least two equations must be available ; i.e., at least three individual sensors must 
trigger to obtain six equations with six unknowns with the assumption that only the 
object position is desired but not the object orientation. 
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constant 



sensor 


fiber optic sensor 

__ ideal sensor 


Figure 18 . Response curves for an optical reflectance sensor and an ideal sensor. 


Noncontact Sensor Models 

Fiber Optic Reflectance Sensor 

There are many different types of noncontact sensors available, each correspond- 
ing to a particular need. Inherent to each sensor is the type of output that it gives. 
For example, the typical output curve for an optical reflectance sensor is shown in 
Figure 18 . This is compared to an ideal sensor, which will be discussed in the next 
section. The output response of an optical reflectance sensor represents diffused light 
intensity from the object to the receiver, which is dependent on the distance, the 
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photometric properties of the object surface, and the local orientation of the object 

(U| 

To realize the complexity of modeling a sensor, Balaure (1986) presents the 
mathematical equations which are described below for the fiber optic reflectance 
sensor shown in Figure 19. The output from the sensor will depend on parameters: 
/i, a, 6, the slant angle of the object, the whiteness of surface of the object, and 
the type of optics used. The received flux is expressed as 


L 0 


A(x,y) B(x,y) 


$(h,X,y) 4 J j D{h 2 +u 2 ){h 2 + v 2 ) 


dx dy 


The variable L 0 is known as the source luminance. The functions A(x,y) and B(x,y) 
are defined as 

u v 

A(x,y) — [cos(2 arctan — — a) + cos a][cos(2 arctan ~-p) + cos a] 

h h 

u v 

B(x^y) — [cos(arctan— — a )][cos( arctan - — a)] 

h h 

where 


,6 2 „ , 

u ~ \l { — + x) 2 + y 2 

(j -*) 2 +» 2 

Of more concern is the detection range of the sensor. The integration area, 
or detection range, of the fiber optic sensor is D , the surface located within the 
intersection of two ellipses. The equations of the ellipses are: 


y l + x 2 (cos 2 a + tan 2 ip sin 2 a) -f x[—8 cos 2 a + h sin 2 a + (8 sin 2 a 

* 2 o ^ 2 8 *y 

+h sin a) tan p\ + (— cos a — h sin a)" — tan" <p(— sin a + h cos a)" = 0 

2 2 


0 9 2 oo r 2 2 2 

3 /“ + z"(cos a + tan" <p sin" a) + [8 cos a — h sin" a — (6 sin a + 

9 2 ] 8 2 *y 8 2 

h sin" a) tan p] + ( — - cos a + h sin a ) — tan" - sin a -f h cos a) 2 = 0. 
L, Z 


and 
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Figure 19. Fiber optic reflectance sensor [13]. 


Ideal Sensor 

An ideal sensor is defined with a spherical detection volume instead of a" “tear- 
shaped” volume as for the fiber optic sensor (Figure 18). This sensor is possible if 
the emitter and receiver lie exactly on top of each other [12]. Another simplification 
is that the target object is chosen as a sphere. Later, a method of implementing the 
procedure for other types of objects, such as cylinders or cubes, is discussed. 

Using Figure 20, the transformation from the sensor frame to the object is 

Sph (7 ,/?, r + R) — Rot(Zs, 7 )Rot(L 5 ,/ 3 )Trans(Zs, r + R) 

cos 7 cos (3 —sin 7 cos 7 sin (3 (r R)c os 7 sin /? 
sin 7 cos (3 cos 7 sin 7 sin/? (r + /?)sin 7 sin (3 

-sin/? 0 cos (3 (r + R) cos /? 

0 0 0 1 


(22) 
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Figure 20. Parameters for the ideal sensor. 


where r is the sensor-to-trigger point distance, R is radius of the sphere, 7 is the 
offset angle in the Xs~Ys plane, and f3 is the ofFset angle from the Z$ axis. 


Examples 
Forward Model 

To find the sensor parameters r, 7 , and (3 of the ideal sensor, it is necessary to 
calculate the transformations required by equation 19, which is rewritten below 

rpobject rppf rr\object 

sensor ^ sensor ^ pf 


Figure 21 shows one finger of the University of Minnesota hand supplied with an ideal 
sensor in close proximity to a sphere. The following matrices can then be computed 


rpsensor 

L pf 


100 1.5 

0 0-10 
0 10 5 

0 0 0 1 



Figure 21. Forward model example using one finger of a dexterous hand 


and the inverse of the matrix above is 

"10 0 -1.5" 

T pf 0 0 1 -5 

sensor Q Q Q 

0 0 0 1 

The transformation from the palm frame to the object frame is 

"l 0 0 1.5" 

rpobject _ 0 1 0 0 

Pf ~ 0 0 1 3 

0 0 0 1 
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and the product T^nsorT^^ 01 is 


T P 1 


object 


sensor A pf 


The last column of the matrix above is the position vector, as shown below 


T *L 


t object 


sensor ^ pf 


'1 

0 

0 

0 

0 

0 

1 

-2 

0 

-1 

0 

0 

0 

0 

0 

1 

e position vector, 

'o' 


' 0 ' 


0 


-2 


0 


0 

■ 

1 


1 



(23) 


The transformation Tensor is 

TZZIt = R • Sph( 7 ,0,r + R ) 


(24) 


where R is a matrix which reorients the x, y, z frame at the sensor to the Xs, 
Zs frame required to find the sensor parameters. The matrix R is then 


R = Rot{x, 90°) = 


1 0 
0 0 

0 1 

0 0 


0 

-1 

0 

0 


0 

0 

0 

1 


Equation 24 becomes 


rpobject 
■*" sensor 


cos 7 cos j3 
sin / 3 

sin 7 cos (3 

0 


— sin 7 cos 7 sin (3 
0 — cos (3 

cos 7 sin 7 sin (3 

0 0 


(r -f R) cos 7 sin (3 
— (r + R) cos 3 
(r + R ) sin 7 sin (3 

1 


(25) 


(26) 


Equate the position vector of the matrix in equation 26 to equation 23 results in three 
equations with three unknowns 


(r + f?) cos 7 sin/3 = 0 (27) 

— (r + R) cos f3 = — 2 (28) 

(r + R) sin 7 sin/3 = 0. (29) 

The solution of equations 27-29 yield 0 = 0 °, 7 = 0°, and 

2 

r = — R = 2 — R — 2 — 1.5 = 0.5 inches. 

cos 0 
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This procedure for finding r, 7 , and (3 for a general three-dimensional case can be 
programmed using a nonlinear least-squares algorithm. 

Inverse Model 


Figure 22 shows two fingers of a dexterous hand surrounding a circle. Recall 
that at least three sensors must trigger for this scheme to work if using ideal sensors. 

The three sensors used to demonstrate the procedure are a sensor from the first 
link and a sensor from the second link of finger 1 , and a sensor from the first link of 


finger 2. Thus equation 21 for this example becomes 


rpsensorurpobject 
pf sensor 11 


rr\SenSOri2rr\object 

sensor 12 



11 


( 5e7i5or2i 

pf 


T 


object 
sensoT2 1 


(30) 


where the first subscript stands for the finger number and the second subscript stands 


for the link number. The following matrices are found: for the first link of finger 1 , 


T 


sensorn 

Pf 


IS 


1 sensor 11 
Pf 


1 0 0 
00-1 
0 1 0 

000 


1.5 

0 
5 

1 


For the second link of finger 1, T 


sensor 12 
Pf 


is 




0.85264 

0.5225 

0 

2.42632' 

rr\SenSOr 12 


0 

0 

-1 

0 

1 pf 


-0.5225 

0.85264 

0 

4.73875 



0 

0 

0 

1 

For the first link of finger 2 

rpsensor2i ■ 

1 p/ 1S 






' 0.70711 

0.70711 

0 

1.35356' 

rpsensor2 1 


0 

0 

-1 

0 

L pf 


-0.70711 

0.70711 

0 

0.64645 



0 

0 

0 

1 

Using equation 24, the tran 

sformations 

rpobject _ r i rp 

-Lsensorii ailQ J_ 

i object 

sensori2 ^re 


rpobject 

sensorij 


R] • Sph(7i j ,^ 1 j,r 1 j + R ) 


( 31 ) 


where j is the link number 1 or 2. The Ri matrix is 

10 0 0 

0 0-10 

0 10 0 

0 0 0 1 


Ri — Rot(x , 90°) = 
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For the first link of finger 2, T°e^ e 3 or 2 i is 


T S 21 = R 2 • Sph( 721 , / 3 21 ,r 21 + R) 


where the R 2 matrix is 


R 2 = Rot(x, —90°) Rot(z, 180°) 


-10 0 0 
0 0 10 

0 10 0 

0 0 0 1 


Then equation 31 becomes 


’cos7ij cos f3\j — sin7i_, COS71J sin/?iy (r\j -f R) cos 71 j sin (3\j 
rpobject _ singly 0 — cos/?ij — (rij -f R) cos f3\j 

sensoTij sin 7 ij cos/3i j cos 71 j sin 7ij sin/?ij [r\j + R) sin 71 j sin (3\j 

000 1 

and (32) becomes 


rpobject 
sensor 21 


— cos 7 2 1 cos / 3 2 i sin 721 — cos 721 sin/3 2 i — (r 2 i + R)cos7 2 i sin/3 2 i 

-sin ^21 0 cos/3 2 i ( r 2i + R) cos /? 2 i 

sin 721 cos /? 2 i cos 721 sin 721 sin /3 2 i (r 2 i + R) sin 721 sin /3 2 i 
0 0 0 1 


Taking the products required by equation 30 and looking at only the last column of 
each, yields 


rr\3€TiSOTnrryobj€ct 0 

^pf sensor u Q 

1 


(rn + R) cos 711 sin/?n + 1.5 
-(rn + R) sin 711 sin/?n 
-(rn + R) cos ySn + 5 

1 


rp3en30Ti2rr\object 0 

“ p / -^sensori 2 Q “ 


0.853(ri2 + R) cos 712 sin /?i 2 — 0.523(ri 2 + R) cos fl \2 + 2.426 
— ( r l2 + -R) sin 712 sin/^12 

— 0. 523(ri2 + R) cos 712 sin flu — 0.853(ri2 + R) cos /?i 2 + 4.739 

1 


rpsensorurjyobject 0 

p/ ■*- sensor2i Q ~ 


-0.707(r 2 i + R) cos 721 sin /3 2 1 + 0.707(r 2 i + R) cos /3 2 i + 1.354 
-(r 2 i + R) sin 721 sin /? 2 i 

0.707(r 2 i + R) cos 721 sin/3 2 i + 0.707(r 2 i + R)cos/3 2 i 4- 0.646 

1 



50 


Table 5. Radial distances, r. 


Variable 

Distances 

(in) 

**11 

0.7097087 

**12 

0.5216860 

r 21 

0.7141319 


Also, from equation 30, the three equations above must all be equal to each other. 
Equating them yields six equations with six unknowns 

7115 011 , 712 , 012 , 721 , 021 - 

Software using a nonlinear least-squares technique was written to solve for the 
unknowns. Table 5 lists the radial distances obtained from the forward model. These 
distances were calculated using the known circle position and finger joint angles. 
Then, the r values were input to the inverse model to calculate the offset angles 
and compared to the values obtained in the forward model, as shown in Table 6 . 
The sensor parameters obtained match those given by the forward model with slight 
differences due to roundoff error. 

Once 7 and (3 are obtained for any sensor, the object frame position with respect 
to the palm frame is the position vector from the product T^ n 5 ° r T sensor for that 
particular sensor. For example, using equation 33 with values obtained from Table 
6 for the sensor on link 1 of finger 1 , the object frame position with respect to the 
palm frame is 

f 1.8125 

rT\ sen90T l\rYobject — 2 X 10 

p/ ± sensor n ~ 2.8125 

1 

which agrees with the input values to the forward solution. 




Table 6. Pose estimation results. 


sensor parameters 

forward solution 
(degrees) 

inverse solution 
(degrees) 

percent error 
(%) 

Til 

% 0 

0.38080 

- 

0ii 

8.1301 

8.1302 

= 0 

712 

= 0 

0.24633 

- 

01 2 

13.8251 

13.8251 

0 

721 

% 0 

= 0 

- 

021 

33.0371 

33.0373 

= o 


The overall procedure can be programed using the nonlinear least-squares 
technique to solve a general three-dimensional problem. 

Discussion 

There are some limitations in the pose estimation algorithm: the fact that only 
the position is calculated versus both the position and orientation, and that the object 
and sensor characteristics are modeled as being spherical. 

To estimate the orientation of the object is a more complex problem. Furhman 
and Kanade (1984) describe a method of estimating the orientation of a surface using 
a multilight proximity sensor. The three-dimensional locations of the spots of light 
on the surface of the object are computed using triangulation. Then, by fitting a 
surface to a set of points, the orientation and curvature of the surface are calculated. 

The pose estimation algorithm used an ideal model for the sensor. A more real- 
istic model of the sensor characteristics is the “tear shaped’ 1 model as shown in Figure 
18. This shape must be modeled mathematically and the equation development must 
take the difference of sensor models into account. 

The pose estimation algorithm also used a spherical model for the target object. 
Other types of objects can be used, but it will require more computation to model 
them. One possible way of representing objects mathematically is to use Fourier 
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Descriptors, which is a method used in imaging processes to represent a shape. The 
problem with using a method such as Fourier Descriptors is that the computation 
time required may make the program run too slow for a real-time environment. 
An alternative would be to treat objects as geometric primitives and develop the 
algorithm to handle these specific primitives. 
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FUTURE RESEARCH 

This chapter discusses future extensions of the investigation. The first section 
presents a grasp strategy using the inverse kinematic and pose estimation algorithms 
to provide low-level control of a dexterous hand when grasping an object. The 
second section lists some ideas for research, software development and simulation, 
and hardware design and implementation of the grasp strategy. 

Grasp Algorithm 

Tomovic, Bekey, and Karplus (1987) proposed a method for grasping arbitrary 
objects using a multifingered hand. They defined the basic elements involved in 
grasping based on the philosophy of reflex control (i.e.; each aspect of the grasping 
task is initiated and terminated using sensory data and rules of behavior derived from 
human expertise). These basic elements are 

1. Represent target objects as geometrical primitives. 

2. Preshape and align the hand to conform with the selected geometrical primitive. 

3. Determine the most suitable hand configuration for the primitive (1, 2, or 3 
fingered grasp). 

4. Separation of the grasping task into a target approach phase and a shape 
adaptation phase while applying reflex control philosophy. 

Keeping these basic elements in mind, a strategy termed the Grasp Algorithm is 
proposed which utilizes the inverse kinematic and pose estimation software to provide 
control of a dexterous hand when grasping an object. Figure 23 shows a flowchart 
for the proposed strategy. First, the vision system will locate and identify the object 
as some geometric primitive and determine its coordinate frame, as shown in box 
1. A strategy similar to the one proposed by Rao, et al (1988) could be used to 
accomplish the task of object identification. Using this information, the robot will 
move towards the object and preshape the hand (box 2). High-level reasoning will 
select the appropriate preshape based on parameters such as object size and shape, 
and the location of the selected grasping points. 
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Next, the robot will position the hand close enough to the object such that 
the object is within the grasping region of the hand (box 3). Data obtained from 
the proximity sensors should be able to answer the question of whether the object 
is within the grasping region. Chammas (1989) discusses a method of obtaining the 
grasping pre-image of a hand, which is the region the object must be to be grasped 
successfully. 

Once the object is within reach, position commands should be sent to the hand 
based on the type of grasp that is required. These position commands are input to 
the inverse kinematics software which determines the joint angles necessary (box 4). 
Now, the object pose is calculated from the proximity sensor data using the pose 
estimation software (box 5), and any adjustments to the finger positions are made 
if necessary. Once sensor outputs reach some threshold, the hand is closed and the 
object is grasped. Vision or tactile sensors can then verify if the object has been 
successfully grasped through measurement of force and slip (boxes 6 and 7). 

Implementation 

This work presented algorithms to determine the inverse kinematics of a ma- 
nipulator and the pose estimation of a pre-grasped object. These algorithms were 
developed assuming several simplifications such as: the inverse kinematics did not 
take into account the mechanical constraints of the manipulator and, as a result, 
multiple solutions were available; the pose estimation algorithm used a spherical sen- 
sor model which may or may not be a close estimation of an actual sensor model. 
Likewise, the object to be grasped was limited to a sphere, and thus orientation did 
not matter. In retrospect, this investigation provided a software shell which can be 
easily upgraded to take into account the factors listed above. 

At present, software is being developed based on the inverse kinematic and pose 
estimation algorithms to simulate the movement of a three-dimensional model of the 
University of Minnesota hand. The modeling software that is being used is called 
TOPAS (AT&T Bell Laboratories). 

Other subjects which should be considered in the future are: 
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Figure 23. Grasping scheme. 
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1. Increase the sensor model complexity of the pose estimation algorithm to match 
the characteristics of actual sensors. 

2. Use actual finger dimensions and other physical constraints to increase useful- 
ness of the inverse kinematics algorithm. 

3. Implement the grasp algorithm in conjunction with a robot hand controller and 
driver to adequately close the hand; i.e,; provide correct timing, force, and form 
of the hand on the object. 

4. Use multisensory data together with the grasp algorithm to successfully manip- 
ulate an object. 
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CONCLUSIONS 

A numerical scheme was developed to solve the inverse kinematics for a user- 
defined manipulator. The scheme was based on a nonlinear least-squares technique 
which determines the joint variables by minimizing the difference between the target 
end effector pose and the actual end effector pose. The scheme was adopted to a 
dexterous hand in which the joints are either prismatic or revolute and the fingers are 
considered open kinematic chains. Feasible solutions were obtained using a three- 
fingered dexterous hand. 

An algorithm to estimate the position and orientation of a pre-grasped object 
was also developed. The algorithm was based on triangulation using an ideal sensor 
and a spherical object model. By choosing the object to be a sphere, only the position 
of the object frame was important. Based on these simplifications, a minimum of three 
sensors are needed to find the position of a sphere. A two dimensional example to 
determine the position of a circle coordinate frame using a two-fingered dexterous 
hand was presented. 
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APPENDIX A 

MAIN PROGRAM FOR INVERSE KINEMATICS 


******************************************************************** 

******************************************************************** 


* PROGRAM MULTI. FOR (LAST REVISION: 9-8-90) * 

* * 

* BY: VICTOR H. PINTO * 

* TEXAS A&M UNIVERSITY, DEPT OF MECHANICAL ENGINEERING * 

* COLLEGE STATION, TEXAS 77843 * 


* PROGRAM DESCRIPTION: 

* THIS PROGRAM CALCULATES THE NECESSARY JOINT ANGLES OF EACH FINGER 

* OF A MULTIFINGERED HAND GIVEN THE KINEMATIC PARAMETERS OF EACH 

* FINGER AND THE DESIRED POSITION OF THE CONTACT-POINT FRAME. 

* THIS TASK IS ACCOMPLISHED USING A LEAST-SqUARES MINIMIZATION 

* PROCESS. 

* HAND MODELED: UNIV. . OF MINN. HAND 

* PARAMETERS: 

* NOL = NUMBER OF LINKS PER FINGER 

* NOF = NUMBER OF FINGERS 

* LDFJAC ,M,N = PARAMETERS USED IN THE LEAST-SQUARES ROUTINE, WHERE 

* M IS THE NUMBER OF FUNCTIONS AND N IS THE NUMBER 

* OF UNKNOWNS . 

* INPUT VARIABLES: 

* AL,AA,DD = MATRICES WHICH CONTAIN THE REQUIRED D-H KINEMATIC 

* PARAMETERS OF EACH FINGER. 

* C1,C2,C3 = MATRICES WHICH DEFINE THE POSITION OF THE 

* CONTACT-POINT WITH RESPECT TO THE OBJECT FRAME. 

* F1,F2,F3 = MATRICES WHICH DEFINE THE TRANSFORMATION FROM 

* THE PALM FRAME TO A FINGER-BASE FRAME. 

* BAR = TRANSFORMATION MATRIX FROM THE PALM FRAME TO THE 

* OBJECT FRAME (IN THIS CASE A RECTANGULAR BAR) . 

* OUTPUT VARIABLES: 

* X = VECTOR OF LENGTH N WHICH CONTAINS THE JOINT ANGLES FOR A 

* PARTICULAR FINGER (RADIANS) 

* XN , YN , ZN = POSITION COOR OF THE FINGERTIP FRAME (INCHES) 

* PHID ,THETAD,PSID = EULER ANGLES OF THE FINGERTIP FRAME (DEGREES) 

* ***NOTE*** NOF AND NOL MUST BE ENTERED INTO THE PARAMETER LIST OF 

* SUBROUTINE FORWARD. 

* ***NOTE*** THE JOINT VARIABLE CONSTRAINTS MUST BE SET IN SUBROUTINE 

* CONSTRAINT. 


* ASSUMPTIONS: 3 FINGERS, 3 LINKS 
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INTEGER LDF JAC , M , N , I , J , NOL , NOF , COUNT , IPARAM (6) , FLAG ,FLAG2 
PARAMETER (LDFJAC=3,M=3,N=3) 

PARAMETER (N0F=3 , N0L=3) 

DOUBLE PRECISION FJAC(LDFJAC.N) ,FSCALE(M) ,FVEC(M) ,RPARAM(7) , 
ft X(N) , XGUESS(N) ,XSCALE(N) ,DEG,POS(3) , XN , YN , ZN , PHID , THETAD , 
ft PSID, PHI, THETA, PSI,AL(NOF, NOL) ,AA(NOF, NOL) ,DD(NOF ,NOL) , 
ft FI (4,4) ,F2(4,4) ,F3(4,4) ,C1(4,4) ,C2(4,4) ,C3(4,4) ,BAR(4,4) , 
ft T(4,4) 

COMMON /CONTACT/ C1,C2,C3 
COMMON /DH.PARAMETERS/ AA , AL , DD 
COMMON /FING.BASE/ F1,F2,F3 
COMMON /BAR/ BAR 
COMMON COUNT, POS,T,XN,YN,ZN 

CHARACTER* 23 OUTPUT 
EXTERNAL DUNLSF .FORWARD 

C ********* ca i cu i a te the joint angles for each finger*********** 

DO 10 C0UNT=1 ,NOF 
C open data files for results: 


20 


WRITE (6,*) ’ENTER OUTPUT FILE NAME FOR FINGER ’.COUNT 
READ (5,20) OUTPUT 
FORMAT (A) 

OPEN (UNIT= 1 5 , FILE=OUTPUT , STATUS= ’ NEW ’ ) 


C call subroutine POSITION to compute the position 
C of the contact -point frame: 


CALL POSITION (COUNT, POS) 

C call IMSL routine to compute least -squares minimization: 

DATA XGUESS /O . 20 , 0 . 20 , 0 . 20/ 

DATA XSCALE /N*1.0/, FSCALE /M*1.0/ 

FLAG2=0 

5 CONTINUE 

IPARAM(l) =0 
FLAG=0 

CALL DUNLSF ( FORWARD ,M,N, XGUESS .XSCALE, FSCALE, IP ARAM 
+ , RPARAM , X , FVEC , F J AC , LDF JAC) 


C check joint variable constraints: 

FLAG2=FLAG2+1 
IF (FLAG2.GT.3) THEN 

WRITE (6,*) ’NO JOINT SOLUTION AVAILABLE FOR 
+FINGER ’.COUNT 
GOTO 10 
ELSE 
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WRITE (6,*) ’ WORKING 
ENDIF 

CALL CONSTRAINT (COUNT, FLAG, X,N) 

C if FLAG is >= 1, then a joint limit has been exceeded. 

IF (FLAG.GT.O) THEN 
DO 6 1=1,6 

XGUESS(I)=X(I) 

6 CONTINUE 
GOTO 5 
ENDIF 

C compute the Euler angles of the contact-point frame: 

IF ((T(l,3) . EQ.O.O) .AND. (T(2,3) .EQ.O.O)) THEN 
PHI=0 . 0 

ELSEIF ( (T(l , 3) .LT.0.0) .AND. (T(2,3) .LT.O.O)) THEN 
PHI=DATAN2 (DABS (T (2 , 3) ) , DABS (T( 1 , 3) ) ) +3 . 14159D0 
ELSE 

PHI=DATAN2 (T (2 , 3) ,T(1,3)) 

END IF 

THETA=DATAN2(DC0S(PHI)*T(i , 3)+DSIN(PHI)*T(2,3) ,T(3,3)) 
PSI=DATAN2 (-DSIN (PHI) *T(l , 1) +DC0S(PHI)*T(2 , 1) .-DSIN(PHI) *T(l , 2) 
++DC0S (PHI) *T(2 , 2) ) 

PHID = PHI * 180 . 0D0/3 . 14159D0 
THETAD = THETA * 180 . 0D0/3 . 14159D0 
PSID = PSI * 180.0D0/3. 14159D0 


C write results to data file: 

WRITE (15,25) COUNT 

25 FORMAT (' ’ ,5X, ’ INVERSE SOLUTION FOR FINGER’ , IX, 12) 

DO 35 J - 1,1 

DEG = X(J) * 180.0D0/3. 14159D0 
WRITE (15,30) COUNT, J, DEG, FVEC(J) 

30 FORMAT (’ ’ , 2X , ’THETA’ , IX , 12 , 12 , IX , ’ = ’ , IX ,F17 . 11 ,4X , 

+’ TOLERANCE =’ , 1X.F15 . 11) 

35 CONTINUE 

WRITE (15,40) COUNT 

40 FORMAT (’ ’, 2X, ’POSITION AND ORIENTATION OF CONTACT FRAME’, IX, 

+12) 

WRITE (15,45) ’ X-DIRECTION = ’ ,XN , ’ Y-DIRECTION =’,YN, 

+ ’ Z-DIRECTION =’ ,ZN 
45 FORMAT (’ » ,2X, A, 2X.F17.il) 

WRITE (15,45) ’PHI =’ ,PHID ,’ THETA =’ .THETAD, ’PSI =’,PSID 
10 CONTINUE 


CL0SE(15) 
CALL EXIT 
END 
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SUBROUTINE TO MINIMIZE RESIDUAL FUNCTIONS 


SUBROUTINE FORWARD (M,N,X,F) 

C THIS SUBROUTINE CALCULATES THE NON-LINEAR FUNCTIONS FOR USE 
C IN THE IMSL ROUTINE DUNLSF. 

INTEGER M,N, I , J .COUNT ,N0F ,N0L 

PARAMETER (N0F=3 , N0L=3) 

DOUBLE PRECISION X(N) ,F(M) ,R(4,4) ,TO(4,4) , T(4,4) ,TEMP(4,4) , 

& FI (4,4) ,F2(4,4) ,F3(4,4) , AL(N0F,N0L) ,AA(NOF,NOL) ,DD(N0F,N0L) , 
& TH(NOF.NOL) ,P0S(3) ,XN,YN,ZN 

COMMON /DH.PARAMETERS/ AA , AL , DD 
COMMON /FING.BASE/ F1.F2.F3 
COMMON COUNT ,POS,T,XN,YN,ZN 


C initialize TO matrix to identity matrix: 


DATA TO /l.,0.,0.,0.,0.,l.,0.,0.,0.,0.,l.,0.,0.,0. ) 0.,l./ 


C initialize variables: 

TH (COUNT , 1 ) =X ( 1 ) 
TH (COUNT, 2)=X(2) 
TH(C0UNT, 3)=X(3) 


C initialize T matrix to identity matrix: 

DO 20 1=1,4 
DO 30 J=1 ,4 

T(I , J) =T0 (I , J) 

30 CONTINUE 

20 CONTINUE 

C compute the transf ormation matrix T for the finger: 


DO 40 1=1, NOL 

CALL TRANSFORM (AL(COUNT.I) ,AA(COUNT,I) ,DD(COUNT,I) , 
+ TH (COUNT, I) ,R) 

CALL MATMULA (T,R) 

40 CONTINUE 


C compute the overall transformation matrix with respect to the 
C hand base frame 



IF ( COUNT. EQ.l) THEN 
DO 50 1=1,4 
DO 60 J=1 ,4 

TEMP (I,J)=F1(I,J) 

60 CONTINUE 

50 CONTINUE 

ELSE IF (COUNT. Eq. 2) THEN 
DO 70 1=1,4 
DO 80 J=i ,4 

TEMP (I, J)=F2(I, J) 

80 CONTINUE 

70 CONTINUE 

ELSE IF (COUNT. Eq. 3) THEN 
DO 90 1=1,4 
DO 100 J=1 ,4 

TEMP (I ,J)=F3(I,J) 
100 CONTINUE 

90 CONTINUE 

END IF 

CALL MATMULA (TEMP ,T) 

DO 110 1=1,4 

DO 120 J-1,4 

T ( I , J ) =TEMP ( I , J ) 

120 CONTINUE 
110 CONTINUE 

C compute position: 


XN=T(1 ,4) 
YN=T(2 ,4) 
ZN=T(3,4) 


C calculate functions: 

F(1)=(P0S(1))-(XN) 
F(2)=(P0S(2))-(YN) 
F (3) =(POS (3))-(ZN) 


C calculate rms values for position errors 

PRMS=DSqRT(F(l)**2+F(2)**2+F(3)**2) 

C print rms values to screen: 

C PRINT *, PRMS 

RETURN 

END 



APPENDIX C 


SUBROUTINE TO CHECK JOINT CONSTRAINTS 


SUBROUTINE CONSTRAINT (COUNT, FLAG, X,N) 

C SUBROUTINE TO VERIFY AND CORRECT THE JOINT VARIABLES 
C IN ORDER TO MAINTAIN MECHANICAL LIMITS. 

INTEGER N, FLAG, COUNT 

DOUBLE PRECISION X(N) , PI, PI2, PI3, PI6 

PI=3 . 14159265D0 
PI2=PI/2 
PI3=PI/3 
PI6=PI/6 

IF ((COUNT. EQ.l) .OR. (COUNT. EQ. 2)) THEN 
IF (X(l) .LT.-PI6) THEN 
X (1 ) =-PI6 
FLAG=FLAG+1 
ENDIF 

IF (X(l) .GT.PI6) THEN 
X (1) =PI6 
FLAG=FLAG+1 
ENDIF 

IF (X(2) .LT.-PI3) THEN 
X(2)=-PI3 
FLAG=FLAG+1 
ENDIF 

IF (X(2) .GT.O.ODO) THEN 
X(2) =0 . ODO 
FLAG=FLAG+1 
ENDIF 

IF (X(3) .LT.-PI2) THEN 
X (3) =-PI2 
FLAG=FLAG+1 
ENDIF 

IF (X(3) .GT.O.ODO) THEN 
X (3) =0 . ODO 
FLAG=FLAG+1 
ENDIF 

ELSEIF (COUNT. EQ. 3) THEN 
IF (X(l) .LT.-PI6) THEN 
X(1)=-PI6 
FLAG=FLAG+1 
ENDIF 

IF (X(l) .GT.PI6) THEN 
X (l) =PI6 
FLAG=FLAG+1 



ENDIF 

IF (X(2) .GT.PI3) THEN 
X(2) =PI3 
FLAG=FLAG+1 
ENDIF 

IF (X(2) .LT.O.ODO) THEN 
X(2) =0 . ODO 
FLAG=FLAG+1 
ENDIF 

IF (X ( 3 ) .GT.PI2) THEN 
X(3) =PI2 
FLAG=FLAG+1 
ENDIF 

IF (X(3) .LT.O.ODO) THEN 
X(3) =0 . ODO 
FLAG=FLAG+1 
ENDIF 
ENDIF 

RETURN 

END 



APPENDIX D 


SUBROUTINE TO CALCULATE 
CONTACT-POINT LOCATION 


SUBROUTINE POSITION (POS) 

C THIS SUBROUTINE CALCULATES THE REQUIRED POSITION OF 
C THE CONTACT-POINT WITH RESPECT TO THE PALM FRAME. 

INTEGER I,J 

DOUBLE PRECISION X(4,4) ,P0S(3) ,CP(3) ,C(4,4) ,BAR(4,4) 

COMMON /CONTACT/ CP 
COMMON /BAR/ BAR 


C initialize matrices: 


100 


DATA C /l. ,0. ,0. ,0. 
DO 100 1=1,3 
C (I , 4) =CP (I) 


O.,l.,0.,0.,0.,0.,l. 1 0.,0.,0.,0.,l./ 


DO 110 1=1,4 
DO 110 J-1,4 
110 X(I,J)=BAR(I,J) 

CALL MATMULA (X,C) 


C retrieve positions: 


POS (1) =X(l ,4) 
POS (2) =X(2 ,4) 
POS (3) =X(3 ,4) 


RETURN 

END 
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APPENDIX E 

SUBROUTINE TO CALCULATE THE A MATRIX 


SUBROUTINE TRANSFORM (AL,AA,DD,TH,TT) 

C SUBROUTINE TO COMPUTE THE INDIVIDUAL T MATRIX 

DOUBLE PRECISION AL , AA , DD , TH , TT (4 , 4) 

TT(1 , 1) =DC0S (TH) 

TT(1 , 2) =-DSIN (TH) *DC0S (AL) 
TT(l,3)=DSIN(TH)*DSIN(AL) 

TT(l ,4) =DC0S (TH) *AA 
TT(2 , 1 ) =DSIN (TH) 

TT(2 ,2) =DC0S (TH) *DC0S (AL) 

TT (2 ,3) =-DC0S (TH) *DSIN(AL) 

TT(2 ,4) =DSIN (TH) *AA 
TT (3,1) *0.0 
TT(3 , 2) =DSIN(AL) 

TT (3 ,3) =DCOS (AL) 

TT (3 ,4) =DD 
TT(4 , 1) =0 . 0 
TT(4,2)=0 .0 
TT(4 ,3) =0 . 0 
TT(4 ,4) =1 . 0 

RETURN 

END 
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APPENDIX F 

SUBROUTINE TO POST-MULTIPLY MATRICES 


SUBROUTINE MATMULA (A,B) 

C SUBROUTINE TO POST-MULTIPLY MATRICES 
INTEGER I.J 

DOUBLE PRECISION A(4,4), B(4,4), C(4,4) 

DO 10 1=1,4 
DO 20 J=1 ,4 

C (I , J)= A(I,1)*B(1,J)+A(I,2)*B(2,J)+A(I,3)*B(3,J) 
++A(1 ,4) *B (4, J) 

20 CONTINUE 

10 CONTINUE 

DO 30 1=1,4 
DO 40 J=1 ,4 

A(I,J)=C(I,J) 

40 CONTINUE 

30 CONTINUE 

RETURN 

END 
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APPENDIX G 

DATA FOR IKP EXAMPLE USING UNIV. OF MINN. HAND 


BLOCK DATA 

DOUBLE PRECISION AL(3 ,3) , AA(3 ,3) ,DD(3 ,3) 
DOUBLE PRECISION Fl(4,4) ,F2(4,4) ,F3(4,4) 
DOUBLE PRECISION Cl(4,4) ,C2(4,4) ,C3(4,4) 
DOUBLE PRECISION BAR(4,4) 

COMMON /DH.PARAMETERS/ AA, AL ,DD 
COMMON /FING.BASE/ F1,F2,F3 
COMMON /CONTACT/ C1,C2,C3 
COMMON /BAR/ BAR 

DATA AL /I . 5708D0 , 1 . 5708D0 , 1 . 5708D0, 

+ O.O, O.O, O.O, 

+ 0.0, 0.0, 0.0/ 

DATA AA /I .ODO , 1 . ODO , 1 . ODO , 

+ 1. ODO, 1. ODO, 1. ODO, 

+ 1 .ODO, 1. ODO, 1. ODO/ 

DATA DD /9*0 . 0/ 

DATA FI /I .0,0. 0,0. 0,0.0, 

+ 0 . 0 , 1 . 0 , 0 . 0 , 0 . 0 , 

+ 0 . 0 , 0 . 0 , 1 . 0 , 0 . 0 , 

+ 1. ODO, -1. ODO, 4. ODO, 1.0/ 

DATA F2 /l. 0,0. 0,0. 0,0.0, 

+ 0 . 0 , 1 . 0 , 0 . 0 , 0 . 0 , 

+ 0 . 0 , 0 . 0 , 1 . 0 , 0 . 0 , 

+ 1. ODO, 1. ODO, 4. ODO, 1.0/ 

DATA F3 /O . 7071 IDO ,0 . 0 , -0 . 70711D0 , 0 . 0 , 


+ 

0.0, 1.0, 0.0, 

0 . 0 , 

+ 

0 . 70711D0 ,0 . 0 , 0 . 70711D0 , 

,0.0, 

+ 

0.5D0, 0. ODO, 1. ODO, 

1.0/ 

DATA Cl 

/1 .0,0. 0,0. 0,0.0, 


+ 

0.0, 1.0, 0.0, 0.0, 


+ 

0.0, 0.0,1 .0,0.0, 


+ 

0 . ODO , -1 . ODO ,0 . 5D0 ,1.0/ 



DATA C2 /l. 0,0. 0,0. 0,0.0, 

+ 0.0, 1.0, 0.0, 0.0, 

+ 0 . 0 , 0 . 0 , 1 . 0 , 0 . 0 , 

+ 0. ODO, 1.000,0.500,1.0/ 


DATA 

C3 /l. 0,0. 0,0. 0,0.0, 

+ 

0.0, 1.0, 0.0, 0.0, 

+ 

0.0, 0.0,1 .0,0.0, 

+ 

0 . ODO , 0 . ODO , -0 . 5D0 ,1.0/ 

DATA 

BAR /l. 0,0. 0,0. 0,0.0, 

+ 

0.0,1 .0,0. 0,0.0, 

+ 

0.0, 0.0, 1.0, 0.0, 

+ 

END 

2. ODO, 0. ODO, 2. ODO, 1.0/ 
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MAIN PROGRAM FOR POSE ESTIMATION 


******^********************************************************** 


* PROGRAM POSH. FOR (LAST REVISION: 10-9-90) * 

* * 

* BY: VICTOR H. PINTO * 

* TEXAS A&M UNIVERSITY, DEPT OF MECHANICAL ENGINEERING * 

* COLLEGE STATION, TEXAS 77843 * 


♦♦♦♦♦♦♦♦♦I********************************************************* 

* PROGRAM DESCRIPTION: 

* THIS PROGRAM FINDS THE POSITION OF A SPHERE GIVEN THE SENSOR 

* DISTANCES (INVERSE SOLUTION) . THIS IS ACCOMPLISHED USING A LEAST- 

* SQUARES MINIMIZATION PROCESS. 

* HAND MODELED: UNIV. OF MINN. HAND 

* PARAMETERS: 

* NOL = NUMBER OF LINKS PER FINGER 

* NOF = NUMBER OF FINGERS 

* NOS = NUMBER OF SENSORS PER LINK 

* SSEN = LINK NUMBER OF THE REFERENCE SENSOR 

* LDFJAC,M,N = PARAMETERS USED IN THE LEAST-SQUARES ROUTINE. 

* WHERE M IS THE NUMBER OF FUNCTIONS, AND N IS 

* THE NUMBER OF UNKNOWNS. 

* INPUT VARIABLES: 

* AL , AA ,DD ,TH = MATRICES WHICH CONTAIN THE REQUIRED D-H KINEMATIC 

* PARAMETERS OF EACH FINGER LINK 

* F1,F2,F3 = TRANSFORMATION MATRICES BETWEEN THE PALM FRAME 

* TO THE FINGER BASE FRAME 

* ZETA = (NOF, 2) MATRIX WHICH CONTAINS THE NECESSARY SENSOR FRAME 

* ROTATIONS 

* RS = (NOF, NOL) MATRIX OF SENSOR VALUES 

* XGUESS = VECTOR OF LENGTH N CONTAINING THE INITIAL GUESS FOR 

* THE LEAST-SqUARES ROUTINE. 

* XSCALE = VECTOR OF LENGTH N CONTAINING THE DIAGONAL SCALING 

* MATRIX FOR THE VARIABLES (USED IN LSQ ROUTINE) . ALL 

* ENTRIES HAVE BEEN SET TO 1.0. 

* SGAMMA, GAMMAI, GAMMA J , SBETA .BETAI , BETA J = SENSOR PARAMETERS 

* THAT ARE CALCULATED PRIOR TO FINDING THE OBJECT POSITION 

* OUTPUT VARIABLES: 

* XO , YO , ZO = POSITION COORDINATES OF OBJECT FRAME 

* ***NOTE*** RS (NOF, NOL) AND ZETA(N0F,2) MUST BE DIMENSIONED 

* IN SUBROUTINE FORWARD. ALSO, NOF AND NOL MUST BE ENTERED 

* INTO THE PARAMETER LIST OF SUBROUTINE FORWARD. 
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* ASSUMPTIONS: 3 FINGERS, 3 SENSORS PER FINGER, 1 SENSOR PER LINK 

* SSEN = 1 

INTEGER LDFJAC,M,N,NOL,NOF, NOS, FING, LINK, SENS, IPARAM(6) , 
ft SSEN 

PARAMETER (LDFJAC=6,M=6 ,N=6) 

PARAMETER (N0F=3 ,N0L=3 , N0S=1) 

DOUBLE PRECISION FJAC(LDFJAC ,N) ,FSCALE(M) ,FVEC(M) ,RPARAM(7) , 
ft X(N) , XGUESS (N) , XSCALE(N) , XO , YO , ZO .RADIUS ,R,RS(NOF ,NOL) , 
ft ZETA(NOF ,2) , SGAMMA,GAMMAI , GAMMA J , SBETA .BETAI .BETA J , 
ft AL(NOF.NOL) ,AA(NOF,NOL) ,DD(NOF ,NOL) ,TH(NOF,NOL) ,F1(4,4) , 
ft F2(4,4) ,F3(4,4) 

COMMON /DH.PARAMETERS/ AL,AA,DD 
COMMON /JOINT_ANGLES/ TH 
COMMON /FING.BASE/ F1,F2,F3 
COMMON /SENSOR_ VALUES/ RS 
COMMON /OPARAM/ RADIUS 
COMMON /FINGER/ FING 
COMMON /LINK/ LINK 
COMMON /REF.SENSOR/ SSEN 
COMMON /SENSOR. ORIENT/ ZETA 

CHARACTER* 23 OUTPUT 
EXTERNAL DUNLSF , FORWARD 


C initialize data: 

SSEN = 1 

C open data files for results: 


120 


WRITE (6,*) ’ENTER OUTPUT FILE NAME ’ 
READ (5,120) OUTPUT 
FORMAT (A) 

OPEN (UNIT=1 5 , FILE=OUTPUT , STATUS= ’ NEW ’ ) 


C determine parameters of the object: 


WRITE (6,*) 'ENTER RADIUS OF SPHERE’ 

READ (5,*) RADIUS 

WRITE (15,*) ’ OBJECT TYPE = SPHERE’ 

WRITE (15,*) ’ RADIUS (INCHES)= ’.RADIUS,’ INCHES’ 

C enter sensor values: 

OPEN (UNIT=16,FILE=’ SEN_VAL.DAT’ , STATUS =’ OLD ’ ) 

DO 5 1=1, NOF 
WRITE (15,*) 

WRITE (15,*) ’ FING’, I,’ SENSOR VALUES’ 

DO 6 J=1 , NOL 

READ (16,*) RS ( I , J ) 

WRITE (15,*) ’FING ( ’ , I , ’ ) LINK(’,J,’)= ’, RS(I,J) 
CONTINUE 
CLOSE (16) 



C enter joint angles : 


DO 1 1=1, NOF 

WRITE (6,*) 'ENTER FING',1,' JOINT ANGLES' 

WRITE (15,*) 

WRITE (15,*) 'FING',1,' JOINT ANGLES’ 

DO 2 J=1 ,N0L 

WRITE (6,*) ’ FING( ’ , I , ’ ) LINK(’,J,’)’ 

READ (5,*) TH (I , J) 

WRITE (15,*) ’FING( ’ , I , ’ ) LINK(’,J,’)= ’,TH(I,J) 

2 CONTINUE 

1 CONTINUE 

C *************** calculate object position ******************* 

DO 10 FING=1 ,N0F 
DO 20 LINK=SSEN+1 ,N0L 

C call least -squares IMSL routine: 

IPARAM(l) =0 

C the guesses are arbitrary 

DATA XGUESS /0. 2,0 . 2.0.2.0 .2 ,0.2 ,0.2/ 

DATA XSCALE /N*1.0/, FSCALE /M*1.0/ 

C ***N0TE*** RS(N0F,N0L) AND ZETA(N0F,2) MUST BE DIMENSIONED 
C IN SUBROUTINE FORWARD. ALSO, NOF AND NOL MUST BE ENTERED 
C INTO THE PARAMETER LIST OF SUBROUTINE FORWARD. 

CALL DUNLSF (FORWARD, M,N, XGUESS .XSCALE, FSCALE, IPARAM 
+ , RPARAM , X , FVEC , F J AC , LDF JAC) 


C calculate the sensor parameters based on constraint eqn’s: 


R=RS (FING .LINK) 

CALL TRUE (R,X(3) ,X(4) .GAMMAI .BETAI) 


WRITE (15,*) 

WRITE (15,*) 'FINGER: 
WRITE (15 , *) ’R=',R, 
WRITE (15,*) 'GAMMA = 
WRITE (15,*) 'BETA =’ 


,FING, ’ LINK: ’.LINK 
'INCHES’ 

.GAMMA I* 180 . 0D0/3 . 14159D0 , ’ DEGREES’ 
BETAI*180 . 0D0/3 . 14159D0 , ’ DEGREES ’ 


20 CONTINUE 


R=RS (FING , SSEN) 

CALL TRUE (R,X(1) ,X(2) , SGAMMA , SBETA) 

WRITE (15,*) 

WRITE(15 , *) 'FINGER: ’ ,FING, ’ LINK:', SSEN 
WRITE(15 , *) ’R=’,R, 'INCHES’ 

WRITE (15 , *) ’ GAMMA = ’ , SGAMMA*180 . 0D0/3 . 14159D0 , ’ DEGREES’ 
WRITE(15 , *) 'BETA =’ .SBETA* 180. 0D0/3 . 14159D0 , ’ DEGREES’ 
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CALL OBJ (NOF ,NOL ,RS ,ZETA ,FING , SSEN, SGAMMA , 

+ SBETA ,XO , YO ,ZO) 

WRITE(15 , *) 

WRITE(15 , *) 'POSE OF OBJECT (WITH RESPECT TO THE BASE FRAME)’ 
WRITE (15,*) ' X-COORDINATE’ ,XO, ’ INCHES’ 

WRITE(15 , *) ’ Y-COORDINATE’ ,YO, ’ INCHES’ 

WRITE (15,*) ’ Z-COORDINATE’ ,ZO, ’ INCHES’ 

10 CONTINUE 
CLOSE( 15) 

CALL EXIT 
END 
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APPENDIX I 

SUBROUTINE TO CALCULATE SENSOR PARAMETERS 


SUBROUTINE FORWARD (M,N,X,F) 

C THIS SUBROUTINE CALCULATES THE NON-LINEAR FUNCTIONS FOR USE 
C IN THE IMSL ROUTINE DUNLSF . 


INTEGER M , N , FING , LINK , NOF , NOL , SSEN 
PARAMETER (N0F=3 ,N0L=3) 

DOUBLE PRECISION X(N) ,F(M) ,RS(3,3) 

DOUBLE PRECISION SSMAT(4,4) ,LMATI(4,4) ,LMATJ(4,4) 
DOUBLE PRECISION RADIUS ,ZETA(3 , 2) 

COMMON /OPARAM/ RADIUS 
COMMON /FINGER/ FING 
COMMON /LINK/ LINK 
COMMON /SENSOR. VALUES/ RS 
COMMON /SENSOR. ORIENT/ ZETA 
COMMON /REF.SENSOR/ SSEN 


C calculate sensor to object transf ormations : 

CALL PRODUCT (NOF , NOL ,RS , ZETA ,N , X, SSMAT, LMATI .LMATJ) 


C calculate functions : 


F(l) = SSMAT(l ,4) 
F(2) = SSMAT(2 , 4) 
F (3) = SSMAT(3,4) 
F (4) = SSMAT(1 ,4) 
F(5) = SSMAT(2 ,4) 
F (6) = SSMAT(3,4) 


LMATI (1 ,4) 
LMATI (2 ,4) 
LMATI (3 ,4) 
LMATJ (1,4) 
LMATJ (2, 4) 
LMATJ (3, 4) 


C calculate rms values for position functions: 


PRMS=DSqRT(F(l)**2+F(2)**2+F(3)**2+F(4)**2+ 
+ F (5) **2+F(6) **2) 


C print rms values to screen: 
C PRINT *, PRMS 


RETURN 

END 
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APPENDIX J 

SUBROUTINE TO CALCULATE OBJECT LOCATION 


SUBROUTINE OBJ (NOF , NOL , RS , ZETA , FING , SSEN , SGAMMA , 
ft SBETA ,X0 ,Y0 , ZO) 

C This subroutine calculates the matrix which specifies the 
C transformation from the palm frame to the object frame. 

C The data used is based on values obtained for the reference sensor. 


INTEGER FING, SSEN, NOF, NOL 

DOUBLE PRECISION RADIUS ,X0 ,Y0 , ZO .SGAMMA , SBETA ,ZETA (NOF , 2) , 
ft OBJECT (4, 4) ,R(4,4) ,RS(N0F,N0L) ,Fl(4,4) ,F2(4,4) ,F3(4,4) , 
ft FS(4,4) , AL(3 ,3) ,AA(3,3) ,DD(3,3) ,TH(3,3) 

COMMON /DH.PARAMETERS/ AL , AA , DD 
COMMON /JOINT. ANGLES/ TH 
COMMON /FING.BASE/ F1.F2/F3 
COMMON /OPARAM/ RADIUS 

CALL BASSEN (NOF , NOL , FING , SSEN, OBJECT) 

CALL SENOBJ (NOF ,N0L , RS .FING , SSEN,ZETA(FING , 1) , 
ft ZETA (FING, 2) .SGAMMA, SBETA, R) 

CALL MATMULA (OBJECT, R) 

. XO=OBJECT(l ,4) 

Y0=0BJECT(2,4) 

Z0=0BJECT(3,4) 

RETURN 

END 
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APPENDIX K 

SUBROUTINE TO CALCULATE THE TRANSFORMATION 
FROM THE FINGER BASE FRAME TO THE OBJECT FRAME 


SUBROUTINE PRODUCT (NOF, NOL, RS, ZETA, N,X, SSMAT, 
+ LMATI, LMATJ) 


C This subroutine calculates the product of: (trans . from the finger 
C base frame to the sensor frame) * (sensor frame orientation matrix) 
C * (Sph(gamma,beta,r+R) . 


INTEGER I , J , N , FING , LINK , SSEN , TEMP , NOF , NOL 
DOUBLE PRECISION X(N) ,ZETA(N0F,2) ,R(4,4) 

DOUBLE PRECISION RADIUS ,RS (NOF, NOL) 

DOUBLE PRECISION SSMAT(4,4) ,LMATI(4,4) ,LMATJ(4,4) 

COMMON /OPARAM/ RADIUS 

COMMON /FINGER/ FING 

COMMON /LINK/ LINK 

COMMON /REF.SENSOR/ SSEN 

TEMP = FING 

CALL SENOBJ (NOF ,N0L ,RS , TEMP , SSEN, ZETA (TEMP , 1) , 
k ZETA (TEMP, 2) ,X(l) ,X(2) ,R) 

CALL BASSEN (NOF, NOL, TEMP, SSEN, SSMAT) 

CALL MATMULA (SSMAT, R) 

CALL SENOBJ (NOF , NOL ,RS , TEMP , LINK , ZETA (TEMP , 1) , 
k ZETA (TEMP, 2) ,X(3) ,X(4) ,R) 

CALL BASSEN (NOF, NOL, TEMP, L INK, LMATI) 

CALL MATMULA (LMATI, R) 

TEMP = TEMP + 1 
IF (TEMP. GT. NOF) THEN 
TEMP = 1 
ENDIF 

CALL SENOBJ (NOF , NOL , RS .TEMP , LINK , ZETA (TEMP , 1 ) , 
k ZETA (TEMP, 2) ,X(5),X(6),R) 

CALL BASSEN (NOF , NOL , TEMP .LINK , LMATJ) 

CALL MATMULA (LMATJ, R) 

RETURN 

END 
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APPENDIX L 

SUBROUTINE TO CALCULATE THE TRANSFORMATION 
FROM THE SENSOR FRAME TO THE OBJECT FRAME 


SUBROUTINE SENOBJ (NOF ,N0L ,RS ,FING ,LINK,THETA1 , 
& THETA2, GAMMA, BETA, ROTX) 

INTEGER FING, LINK, NOF, NOL 

DOUBLE PRECISION R0TX(4,4) ,R0TZ(4,4) ,PR0D3(4,4) 
DOUBLE PRECISION GAMMA, BETA, THETA1 .THETA2 
DOUBLE PRECISION RS(N0F , NOL) .RADIUS 
COMMON /OPARAM/ RADIUS 


C calculate sensor frame orientation: 

R0TX(1 , 1) =1 . 0 
ROTX (1 , 2) =0 . 0 
R0TX(1 ,3) =0.0 
R0TX(1 ,4)=0.0 
R0TX(2 , 1)=0.0 
ROTX (2 , 2) =DC0S (THETAl) 

ROTX (2 , 3) =-DS IN (THETAl) 

ROTX (2, 4) =0.0 
R0TX(3 , 1) =0 . 0 
ROTX (3 , 2) =DSIN (THETAl) 

ROTX (3 , 3) =DCOS (THETAl ) 

ROTX (3 ,4) =0 . 0 
ROTX (4,1) =0.0 
ROTX (4, 2) =0.0 
ROTX (4, 3) =0.0 
ROTX (4, 4) =1 . 0 

ROTZ (1,1) =DCOS (THETA2 ) 

R0TZ(1 , 2) =-DSIN (THETA2) 

ROTZ (1,3) =0.0 
ROTZ(l ,4) =0.0 
ROTZ (2 , 1 ) =DS IN (THETA2) 

ROTZ (2 , 2) =DCOS (THETA 2 ) 

ROTZ (2, 3) =0.0 
ROTZ (2 ,4) =0 . 0 
ROTZ (3 , 1) =0 . 0 
ROTZ (3, 2) =0.0 
ROTZ (3 , 3) = 1 . 0 
ROTZ (3, 4) =0.0 
R0TZ(4 , l) =0 . 0 
ROTZ (4, 2) =0.0 
ROTZ (4, 3) =0.0 
ROTZ (4 , 4) =1 . 0 
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C calculate Sph (gamma, beta, r+R) : 

PR0D3( 1 , 1 ) =DCOS (GAMMA) *DCOS (BETA) 

PR0D3(1 ,2)=-DSIN(GAMMA) 

PR0D3 ( 1 , 3) =DCOS (GAMMA) *DSIN (BETA) 

PR0D3(1 ,4)=(RS(FING,LINK) +RADIUS)*DCOS (GAMMA) *DSIN (BETA) 
PR0D3 (2 , 1 ) =DSIN(GAMMA) *DCOS (BETA) 

PR0D3(2 , 2)=DC0S(GAMMA) 

PR0D3 (2 , 3) =DSIN (GAMMA) *DSIN(BETA) 

PR0D3 (2 ,4) = (RS (FING .LINK) + RADIUS) *DS IN (GAMMA) *DSIN(BETA) 
PR0D3(3 , 1) =-DSIN(BETA) 

PR0D3(3 , 2) =0 . 0 
PRQD3 (3 , 3) =DC0S (BETA) 

PR0D3 (3 , 4) = (RS (FING .LINK) +RADIUS) *DC0S (BETA) 

PR0D3(4 , 1) =0 . 0 
PROD3(4,2)=0.0 
PROD3(4,3)=0.0 
PR0D3(4,4)=1 . 0 

CALL MATMULA (R0TX.R0TZ) 

CALL MATMULA (R0TX.PR0D3) 

RETURN 

END 
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APPENDIX M 


SUBROUTINE TO CALCULATE THE TRANSFORMATION 
FROM THE FINGER BASE FRAME TO THE SENSOR FRAME 


SUBROUTINE BASSEN (NOF ,N0L ,FING , LINK .PROD) 

C This subroutine computes the product of the finger base matrix F 
C and the sensor transformation matrix. 

INTEGER FING, LINK, NOF, NOL 

DOUBLE PRECISION PR0D(4,4) ,FLPR0D(4,4) ,TEMP(4,4) , 

& FI (4,4) ,F2(4,4) ,F3(4,4) ,FS(4,4) ,AL(3,3) ,AA(3,3) , 
a DD (3,3) ,TH(3 ,3) ,R(4,4) 

COMMON /DH.PARAMETERS/ AL , AA , DD 
COMMON /JOINT.ANGLES/ TH 
COMMON /FING.BASE/ F1,F2,F3 

IF (FING.EQ. 1) THEN 
DO 10 1=1,4 
DO 20 J-1,4 
PR0D(I , J)=F1 (I , J) 

20 CONTINUE 

10 CONTINUE 

ELSEIF (FING.EQ. 2) THEN 
DO 30 1=1,4 
DO 40 J=1 ,4 
PR0D(I , J)=F2(I , J) 

40 CONTINUE 

30 CONTINUE 

ELSEIF (FING.EQ. 3) THEN 
DO 50 1=1,4 
DO 60 J=1 ,4 
PROD (I ,J)=F3(I,J) 

60 CONTINUE 

50 CONTINUE 

END IF 

C calculate transf ormation from the finger base frame to the link 
C frame specified by FING and LINK: 

DATA TEMP /I . ,0 . , 0 . ,0 . , 0 . , 1 . , 0 . , 0 . ,0 . ,0 . , 1 . ,0 . , 0 . ,0 . , 0 . , 1 . / 
DO 70 1=1,4 
DO 80 K=1 ,4 

FLPROD ( I , K ) =TEMP ( I , K ) 

80 CONTINUE 

70 CONTINUE 


C compute link matrix: 
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DO 90 J=1 , LINK 

CALL TRANSFORM (AL (FING , J) , AA(FING , J) ,DD(FING , J) , 
+ TH(FING.J) ,R) 

CALL MATMULA (FLPROD ,R) 

90 CONTINUE 

CALL MATMULA (PROD .FLPROD) 


C compute FS matrix: 

FS (1 , 1 ) =1 . 0 
FS(l ,2)=0 .0 
FS (1 ,3) =0 . 0 

FS ( 1 ,4)=-AA(FING , LINK) *0 . 5 

FS(2 , 1 ) =0 . 0 

FS(2,2) = 1 .0 

FS(2,3)=0.0 

FS (2 ,4) =0 . 0 

FS (3 , 1 ) =0 . 0 

FS (3 , 2) =0 . 0 

FS (3 , 3) = 1 . 0 

FS(3,4)=0.0 

FS(4,1)=0.0 

FS (4 , 2) =0 . 0 

FS(4,3)=0.0 

FS(4,4) = 1 .0 


C compute PROD matrix: 

CALL MATMULA (PROD.FS) 

RETURN 

END 
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APPENDIX N 

SUBROUTINE TO CALCULATE THE TRANSFORMATION 
FROM THE SENSOR FRAME TO THE TRIGGER POINT 


SUBROUTINE TRUE (RS , GAMMA .BETA , GAMMAS .BETAS) 

DOUBLE PRECISION RS, GAMMA, BETA, FS(4, 4) .GAMMAS, BETAS 


C compute the transformation from the sensor frame to the 
C trigger point : 


FS (1 , 1) =1 . 0 
FS (1 , 2) =0 . 0 
FS (1 ,3) =0 . 0 

FS (1 ,4) =RS*DC0S (GAMMA) *DSIN(BETA) 

FS (2 , 1) =0 . 0 
FS (2 , 2) =1 . 0 
FS (2 , 3) =0 . 0 

FS (2 ,4) =RS*DSIN (GAMMA) *DSIN(BETA) 

FS (3,1) =0.0 
FS (3 , 2) =0 . 0 
FS (3 ,3) =1 . 0 
FS (3,4) =RS*DC0S (BETA) 

FS (4,1) =0.0 
FS (4 , 2) =0 . 0 
FS (4 ,3) =0 . 0 
FS (4 ,4) =1 . 0 

IF ((FS(1 ,4) .EQ .0.0) .AND. (FS(2,4) .EQ.0.0)) THEN 
GAMMAS=0 . 0 

ELSEIF ((FS(1,4) .LT.0.0) .AND. (FS(2,4) .LT.0.0)) THEN 
GAMMAS=DATAN2 (DABS (FS (2 ,4) ) , DABS(FS(l ,4) ) ) +3 . 14159D0 
ELSE 

GAMMAS=DATAN2 (FS (2 ,4) ,FS(l,4)) 

ENDIF 

IF (GAMMAS. LT.0.0) THEN 

GAMMAS=GAMMAS+2D0*3. 14159D0 
ENDIF 

BETAS=DATAN2(FS(1 ,4) *DC0S (GAMMAS) +FS (2,4) *DSIN(GAMMAS) ,FS(3 ,4) ) 
RS=DSIN (BETAS)* (FS (1 ,4) *DC0S(GAMMAS)+FS(2,4)*DSIN(GAMMAS) ) + 

+FS (3 ,4) *DC0S (BETAS) 

RETURN 

END 



APPENDIX O 


DATA FOR POSE ESTIMATION EXAMPLE 


BLOCK DATA 

DOUBLE PRECISION AL(3 ,3) , AA(3 ,3) ,DD(3 ,3) 

DOUBLE PRECISION FI (4 ,4) ,F2(4,4) , F3(4,4) ,ZETA(3 , 2) 

COMMON /DH_PARA METERS/ AL , AA , DD 

COMMON /FING.BASE/ F1.F2.F3 

COMMON /SENS0R_ ORIENT/ ZETA 

DATA AL / 1 . 5708D0 , 1 . 5708D0 , 1 . 5708D0 , 

+ 0 . 0 , 0 . 0 , 0 . 0 , 

+ 0 . 0 , 0 . 0 , 0 . 0 / 

DATA AA / 1 . ODO , 1 . ODO , 1 . ODO , 

+ 1. ODO, 1. ODO, 1. ODO, 

+ 1. ODO, 1. ODO, 1. ODO/ 

DATA DD /9*0 . 0/ 

DATA FI /l. 0,0. 0,0. 0,0.0, 

+ 0.0, 1.0, 0.0, 0.0, 

+ 0.0, 0.0, 1.0, 0.0, 

+ 1. ODO, -1. ODO, 5. ODO, 1.0/ 

DATA F2 /l. 0,0. 0,0. 0,0.0, 

+ 0 . 0 , 1 . 0 , 0 . 0 , 0 . 0 , 

+ 0.0, 0.0, 1.0, 0.0, 

+ 1. ODO, 1. ODO, 5. ODO, 1.0/ 

DATA F3 / 0 . 7071 IDO ,0 .0,-0. 7071 IDO ,0. 0 , 

+ 0 - 0 , 1 . 0 , 0 . 0 , 0 . 0 , 

+ 0 .7071 IDO ,0.0, O.7O711DO.0.O, 

f 1.0, 0.0, 1.0, 1.0/ 

DATA ZETA /1 . 57080D0 , 1 . 57080D0, -1 . 57080DO, 
f O.ODO, O.ODO, 3 . 14159D0/ 

END 



